#include "GeometryTools.h"

GeometryTools::GeometryTools(void)
{
}

GeometryTools::~GeometryTools(void)
{
}

bool GeometryTools::isInsideOfTriangle(TriMesh::Point &p0, TriMesh::Point &p1, TriMesh::Point &p2, float p[])
{
	double v[2] = {p[0], p[1]};
	double v0[2] = {p0[0], p0[1]};
	double v1[2] = {p1[0]-p0[0], p1[1]-p0[1]};
	double v2[2] = {p2[0]-p0[0], p2[1]-p0[1]};
	double a = ((v[0]*v2[1]-v[1]*v2[0])-(v0[0]*v2[1]-v0[1]*v2[0]))/(v1[0]*v2[1]-v1[1]*v2[0]);
	double b = -((v[0]*v1[1]-v[1]*v1[0])-(v0[0]*v1[1]-v0[1]*v1[0]))/(v1[0]*v2[1]-v1[1]*v2[0]);
	return (a>0&&b>0&&(a+b)<1);
};

bool GeometryTools::isInsideOfTriangle(TriMesh::Point &p0, TriMesh::Point &p1, TriMesh::Point &p2, double p[])
{
	double v[2] = {p[0], p[1]};
	double v0[2] = {p0[0], p0[1]};
	double v1[2] = {p1[0]-p0[0], p1[1]-p0[1]};
	double v2[2] = {p2[0]-p0[0], p2[1]-p0[1]};
	double a = ((v[0]*v2[1]-v[1]*v2[0])-(v0[0]*v2[1]-v0[1]*v2[0]))/(v1[0]*v2[1]-v1[1]*v2[0]);
	double b = -((v[0]*v1[1]-v[1]*v1[0])-(v0[0]*v1[1]-v0[1]*v1[0]))/(v1[0]*v2[1]-v1[1]*v2[0]);
	return (a>0&&b>0&&(a+b)<1);
};

double GeometryTools::distPoint2Line(TriMesh::Point &p1, TriMesh::Point &p2, float p[])
{
	return abs((p2[0]-p1[0])*(p1[1]-p[1]) - (p1[0]-p[0])*(p2[1]-p1[1]))/sqrt((p2[0]-p1[0])*(p2[0]-p1[0])+(p2[1]-p1[1])*(p2[1]-p1[1]));
};

double GeometryTools::distPoint2Line(TriMesh::Point &p1, TriMesh::Point &p2, double p[])
{
	return abs((p2[0]-p1[0])*(p1[1]-p[1]) - (p1[0]-p[0])*(p2[1]-p1[1]))/sqrt((p2[0]-p1[0])*(p2[0]-p1[0])+(p2[1]-p1[1])*(p2[1]-p1[1]));
};

double GeometryTools::distPoint2Plane(double plane[4], TriMesh::Point &p)
{
	return (plane[0]*p[0]+plane[1]*p[1]+plane[2]*p[2]+plane[3]) / sqrt(plane[0]*plane[0]+plane[1]*plane[1]+plane[2]*plane[2]);
}
/*
void GeometryTools::spanPlane(qglviewer::Vec &p1, qglviewer::Vec &p2, qglviewer::Vec &p3, double result[4])
{
	TriMesh::Point v1;
	TriMesh::Point v2;
	TriMesh::Point v3;
	v1[0] = p1.x; v1[1] = p1.y; v1[2] = p1.z;
	v2[0] = p2.x; v2[1] = p2.y; v2[2] = p2.z;
	v3[0] = p3.x; v3[1] = p3.y; v3[2] = p3.z;
	spanPlane(v1,v2,v3,result);
};*/

void GeometryTools::spanPlane(TriMesh::Point &p1, TriMesh::Point &p2, TriMesh::Point &p3, double result[4])
{
	TriMesh::Point plane = ((p2-p1)%(p3-p1)) / ((p2-p1).length() * (p3-p1).length());
	result[0] = plane[0];
	result[1] = plane[1];
	result[2] = plane[2];
	result[3] = -(result[0]*p1[0] + result[1]*p1[1] + result[2]*p1[2]);
}

void GeometryTools::projectPoint2Plane(double plane[4], TriMesh::Point &p1, TriMesh::Point &p)
{
	double denom = plane[0]*plane[0] + plane[1]*plane[1] + plane[2]*plane[2];
	double mu = -(plane[3]+plane[0]*p1[0]+plane[1]*p1[1]+plane[2]*p1[2]) / denom;
	p[0] = p1[0] + mu*plane[0];
	p[1] = p1[1] + mu*plane[1];
	p[2] = p1[2] + mu*plane[2];
};

int GeometryTools::isInsideOfPolygon(int nvert, float *vertx, float *verty, float testx, float testy)
{
	int i, j, c = 0;
	for (i = 0, j = nvert-1; i < nvert; j = i++) {
		if ( ((verty[i]>testy) != (verty[j]>testy)) &&
			(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
			c = !c;
	}
	return c;
}

double GeometryTools::radiusOfInscribedCircle(TriMesh *mesh, TriMesh::FaceHandle &fh)
{
	TriMesh::FaceVertexIter fv_it = mesh->fv_iter(fh);
	TriMesh::Point p1 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p2 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p3 = mesh->point(fv_it);
	double a = (p1-p2).length();
	double b = (p2-p3).length();
	double c = (p3-p1).length();
	double s = (a+b+c)/2;
	return sqrt((s-a)*(s-b)*(s-c)/s);
}

double GeometryTools:: radiusOfCircumscribedCircle(TriMesh *mesh, TriMesh::FaceHandle &fh)
{
	TriMesh::FaceVertexIter fv_it = mesh->fv_iter(fh);
	TriMesh::Point p1 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p2 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p3 = mesh->point(fv_it);
	double a = (p1-p2).length();
	double b = (p2-p3).length();
	double c = (p3-p1).length();
	return a*b*c/sqrt((a+b+c)*(-a+b+c)*(a-b+c)*(a+b-c));
}

double GeometryTools::minAngleInTriangle(TriMesh *mesh, TriMesh::FaceHandle &fh)
{
	TriMesh::FaceVertexIter fv_it = mesh->fv_iter(fh);
	TriMesh::Point p1 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p2 = mesh->point(fv_it);
	++fv_it;
	TriMesh::Point p3 = mesh->point(fv_it);
	double minAngle = 0;
	double angle1 = acos((p1-p2)|(p3-p2) / ((p1-p2).length()*(p3-p2).length()));
	double angle2 = acos((p2-p1)|(p3-p1) / ((p2-p1).length()*(p3-p1).length()));
	double angle3 = acos((p1-p3)|(p2-p3) / ((p1-p3).length()*(p2-p3).length()));
	minAngle = angle1<angle2 ? angle1:angle2;
	minAngle = minAngle<angle3 ? minAngle:angle3;
	return minAngle;
}
